﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace VRP_Saving_Final
{
    //this iterates through the contraints/feasibility checks and merges when appropriate
    class Saving
    {
        public static void Clark_Wright(VRP network)
        {
            int numnodes = network.numnodes;
            float[,] costmatrix = (float[,])network.costmatrix;
            float[] demandarray = network.demandarray;
            float capacity = network.capacity;
            float lambda = network.lambda;
            List<Nodes> nodes = new List<Nodes>();
            nodes.Add(new Nodes());
            for (int i = 1; i < numnodes; i++)          //initialize nodes
            {
                nodes.Add(new Nodes());
                nodes[i].demand = demandarray[i];       //set demand
                nodes[i].assigned = false;          //initially unassigned
                nodes[i].firstonroute = false;          //initially not on route
                nodes[i].lastonroute = false;           //initially not on route
                nodes[i].routeon = 0;                   //initially not on route
                nodes[i].interior = false;          //initially not on route
            }
            //This first part calculates the savings matrix savings[i,j] = costmatrix[0,i] + costmatrix [0,j] - lambda*costmatrix[i,j] for all i, j < N.  
            
            
            int numsavings = ((numnodes - 1) * (numnodes - 1) - (numnodes - 1)) / 2;                //for n nodes, there are (n^2 - n)/2 savings calculations
            float[,] savingsmatrix = new float[numsavings, 3];                                      //this is matrix, where col1 = savings, col2 = i, col3 = j
            int row = -1;                                                           //track index of row working on in savings matrix, so savings, i, and j are all on same row in matrix

            for (int i = 1; i < numnodes; i++)                                  
            {
                for (int j = i + 1; j < numnodes; j++)                  //calculates the savings value for each i, j combo
                {
                                                                                
                    float savingsvalue = costmatrix[0, i] + costmatrix[0, j] - lambda * costmatrix[i, j];
                    row = row + 1;                                                          //iterates through each row from 0 to (n^2 - n)/2 
                    savingsmatrix.SetValue(savingsvalue, row, 0);                               //savings value goes in place (row,0)
                    float ifloat = (float)i;
                    savingsmatrix.SetValue(ifloat, row, 1);                                 //i value goes in place (row,1)
                    float jfloat = (float)j;
                    savingsmatrix.SetValue(jfloat, row, 2);                                         //j value goes in place (row,2)
                }
            }


            Sorting.Sort(savingsmatrix, 0);                                             //sorts ascending, so least savings is in the top row, and most saving is in the bottom row
          
             //creates a routes matrix where 0-i-0 initial route is each row of the list of lists


            List<List<int>> initialroutes = new List<List<int>>();
            List<int> nullroute = new List<int>();
            initialroutes.Add(nullroute);                                           //add a null route in location 0, since the depot isn't on a route
            //for (int k = 1; k <= numnodes; k++)
            for (int k = 1; k <= numnodes; k++)
            {
                List<int> singleroute = new List<int>();
                singleroute.Add(k);
                initialroutes.Add(singleroute);

                //check feasibility of route's capacity
                if (demandarray[k-1] > capacity)
                {
                    Console.WriteLine("Infeasible, single demand node is greater than vehicle capacity");
                }

            }
   

            //creates a routes list of list for the assigned routes

            List<List<int>> routes = new List<List<int>>();
            List<int> emptyroute = new List<int>();
            routes.Add(emptyroute);
            int indexofnewroute = 0;

            //feasibility checks & merging routes that are feasible
            for (int i = numsavings - 1; i >= 0; i--)                   //go through each combo in the saving's matrix
            {
                float currentifloat = savingsmatrix[i, 1];
                int currenti = (int)currentifloat;                      //stores the i in the current location in the saving's matrix
                float currentjfloat = savingsmatrix[i, 2];
                int currentj = (int)currentjfloat;                      //stores the i in the current savings
                       

                bool feasible1 = false;                     //clear all feasiblity checks and cases for routes
                bool feasible2 = false;
                bool feasible3 = false;
                int case1 = 0;
                int case2 = 0;
                int case3 = 0;
                int case4 = 0;

                if (nodes[currenti].assigned == false && nodes[currentj].assigned == false)         //both i and j are unassigned to a route (still on intial routes)
                { case1 = 1;}

                if (nodes[currenti].assigned == false && nodes[currentj].assigned == true)          //i is unassigned and j is on a route
                { case2 = 1;}

                if (nodes[currenti].assigned == true && nodes[currentj].assigned == false)        //i is unassigned and j is on a route
                { case3 = 1;}

                if (nodes[currenti].assigned == true && nodes[currentj].assigned == true)           //both are assigned to a route already 
                { case4 = 1;}

                //both unassigned (both on initial routes)
                if (case1 == 1)
                {
                    //feasibility check for demand only and merge if satisfied

                    if (nodes[currenti].demand + nodes[currentj].demand <= capacity)        
                    {
                        List<int> newroute = new List<int>();                   //create new list for the newly merged nodes
                        newroute.Add(currenti);                                 //add i and j to this new list
                        newroute.Add(currentj);
                        routes.Add(newroute);                                   //add this route to the list of routes we are storing
                        //update
                        nodes[currenti].routeon = indexofnewroute+1;              //assign the node's routeon characteristic to the new route's index value
                        nodes[currentj].routeon = indexofnewroute+1;
                        nodes[currenti].assigned = true;                        //now i and j are assigned to a route
                        nodes[currentj].assigned = true;
                        nodes[currenti].firstonroute = true;                    //i is the first on the route, j is the last on the route (there are only two on this route)
                        nodes[currentj].lastonroute = true;
                        indexofnewroute = indexofnewroute + 1;

                    }
                }


                //if j is assigned , i is not assigned to any route (still on initial)
                if (case2 ==1)
                {
                    if (nodes[currentj].interior == false)              //make sure j is not interior on it's present 
                    {
                        feasible2 = true;
                    }

                    float totaldemand = 0f;                                     //initialize total demand to 0
                    routes[nodes[currentj].routeon].ForEach(delegate(int value) { totaldemand = totaldemand + nodes[value].demand; });      //interalize sums demand on j's current route
                    totaldemand = totaldemand + nodes[currenti].demand;             //add's i's demand to the total of j's route

                    if (feasible2 == true && totaldemand <= capacity)           //only merge if j isn't inteior and not over capacity
                    {
                        if (nodes[currentj].firstonroute == true)               //if current j is first on route...
                        {
                            routes[nodes[currentj].routeon].Insert(0, currenti);            //add i to be the new first on route
                            //update
                            int jon = nodes[currentj].routeon;                          //record j's current route it is on
                            nodes[currenti].routeon = jon;                              //update i and j attributes
                            nodes[currenti].assigned = true;
                            nodes[currenti].firstonroute = true;
                            nodes[currenti].lastonroute = false;
                            nodes[currenti].interior = false;
                            nodes[currentj].firstonroute = false;
                            nodes[currentj].lastonroute = false;
                            nodes[currentj].interior = true;

                        }

                        else
                        {
                            routes[nodes[currentj].routeon].Add(currenti);    //if j is last on route, add i to the end
                            //update
                            int jon = nodes[currentj].routeon;          //record j's current route it is on and update i and j attributes
                            nodes[currenti].routeon = jon;
                            nodes[currenti].assigned = true;
                            nodes[currenti].lastonroute = true;
                            nodes[currenti].firstonroute = false;
                            nodes[currenti].interior = false;
                            nodes[currentj].lastonroute = false;
                            nodes[currentj].firstonroute = false;
                            nodes[currentj].interior = true;
                        }
                    }

                }

                if (case3 == 1)                                         //if i is assigned and j is unassigned
                {
                    if (nodes[currenti].interior == false)          //interior check
                    {
                        feasible2 = true;
                    }

                    float totaldemand = 0f;         
                    routes[nodes[currenti].routeon].ForEach(delegate(int value) { totaldemand = totaldemand + nodes[value].demand; });      //compute total demand for capcacity check of potential new route
                    totaldemand = totaldemand + nodes[currentj].demand;

                    if (feasible2 == true && totaldemand <= capacity)       //merge only if capacity/interior satistifed
                    {
                        if (nodes[currenti].firstonroute == true) 
                        {
                            routes[nodes[currenti].routeon].Insert(0, currentj);        //put j at beginning if i is first on route
                            //update
                            int ion = nodes[currenti].routeon;
                            nodes[currentj].routeon = ion;
                            nodes[currentj].assigned = true;
                            nodes[currentj].firstonroute = true;
                            nodes[currentj].lastonroute = false;
                            nodes[currentj].interior = false;
                            nodes[currenti].lastonroute = false;
                            nodes[currenti].interior = true;
                            nodes[currenti].firstonroute = false;


                        }
                        else                                              //put j at end if i is last on route
                        {
                            routes[nodes[currenti].routeon].Add(currentj);
                            //update
                            int ion = nodes[currenti].routeon;
                            nodes[currentj].routeon = ion;
                            nodes[currentj].assigned = true;
                            nodes[currentj].lastonroute = true;
                            nodes[currentj].firstonroute = false;
                            nodes[currentj].interior = false;
                            nodes[currenti].lastonroute = false;
                            nodes[currenti].interior = true;
                            nodes[currenti].firstonroute = false;
                        }

                    }

                }


                //if both assigned to a route already
                if (case4 == 1)
                {
                    //feasibility check 1, to see if the nodes i, j are on not on the same route (or both unassigned)
                    if (nodes[currenti].routeon != nodes[currentj].routeon)
                    {
                        feasible1 = true;
                    }

                    //feasibility cheack 2, to see if the nodes i,j are at the beginning or end of their respective routes
                    if (nodes[currenti].interior == false && nodes[currentj].interior == false)
                    {
                        feasible2 = true;
                    }

                    //feasbility check 3, to see if the demand is less than capacity of new route
                    float totaldemand = 0f;
                    routes[nodes[currenti].routeon].ForEach(delegate(int value) { totaldemand = totaldemand + nodes[value].demand; });
                    routes[nodes[currentj].routeon].ForEach(delegate(int value) { totaldemand = totaldemand + nodes[value].demand; });
                    if (totaldemand <= capacity)
                    {
                        feasible3 = true;
                    }

                    //merge if all feasibility requirements are satisfied
                    if (feasible1 == true && feasible2 == true && feasible3 == true)
                    {
                        //merge case 1: if both i, j are the final elements of their respective routes
                        if (nodes[currenti].lastonroute == true && nodes[currentj].lastonroute == true)
                        {
                            List<int> oldroute1 = new List<int>(); //current oldroute
                            oldroute1 = routes[nodes[currentj].routeon];// old route is the route that current j is on
                            oldroute1.Reverse(); //reverse it before merging to ensure i next to j
                            for (int k = oldroute1.Count - 1; k >= 0; k--) //for each element on the old route, we add it to the route i is on
                            { routes[nodes[currenti].routeon].Add(oldroute1[k]); }
                            oldroute1.Clear(); //get rid of the old route
                            routes[nodes[currentj].routeon] = oldroute1; //set the location on routes where the old route was located to an empty route
                            //update
                            List<int> newroute1 = new List<int>(); //the new route is the route that we just created, with both i and j
                            newroute1 = routes[nodes[currenti].routeon];//assigns the new route
                            int ion = nodes[currenti].routeon;//record the index that current i (and j) is on
                            newroute1.ForEach(delegate(int value) { nodes[value].routeon = ion; }); //for each element on the new route, update the routeon value
                            foreach (var value in newroute1) //update firston, laston and interior bools
                            {
                                if (newroute1.IndexOf(value) == 0)
                                {
                                    nodes[value].firstonroute = true;
                                }
                                else
                                {
                                    nodes[value].firstonroute = false;
                                }

                                if (newroute1.IndexOf(value) == newroute1.Count-1)
                                {
                                    nodes[value].lastonroute = true;
                                }
                                else
                                {
                                    nodes[value].lastonroute = false;
                                }
                                if (nodes[value].firstonroute == false && nodes[value].lastonroute == false)
                                {
                                    nodes[value].interior = true;
                                }
                                else
                                {
                                    nodes[value].interior = false;
                                }
                            }
                        }
                        //merge case 2: if node i is last on route, and node j is first on route
                        if (nodes[currenti].lastonroute == true && nodes[currentj].firstonroute == true)
                        {
                            List<int> oldroute2 = new List<int>(); //current old route
                            oldroute2 = routes[nodes[currentj].routeon]; //set current old route the the route j is on
                            for (int k = oldroute2.Count - 1; k >= 0; k--) //for each element on the old route, we add it to the end of the route i is on
                            { routes[nodes[currenti].routeon].Add(oldroute2[k]); }
                            oldroute2.Clear();//clear old route 2
                            routes[nodes[currentj].routeon] = oldroute2;//assign the oldroute2 (now empty) to the routes in that locations
                            //update
                            List<int> newroute2 = new List<int>(); //the new route is the merged routes with i and j
                            newroute2 = routes[nodes[currenti].routeon]; //assign new route 2
                            int ion = nodes[currenti].routeon; //record the index of the new route (based on the current index of i
                            newroute2.ForEach(delegate(int value) { nodes[value].routeon = ion; }); //for each element in new route 2, update the routeon
                            foreach (var value in newroute2) //update firston, laston and interior bools
                            {
                                if (newroute2.IndexOf(value) == 0)
                                {
                                    nodes[value].firstonroute = true;
                                }
                                else
                                {
                                    nodes[value].firstonroute = false;
                                }

                                if (newroute2.IndexOf(value) == newroute2.Count-1)
                                {
                                    nodes[value].lastonroute = true;
                                }
                                else
                                {
                                    nodes[value].lastonroute = false;
                                }
                                if (nodes[value].firstonroute == false && nodes[value].lastonroute == false)
                                {
                                    nodes[value].interior = true;
                                }
                                else
                                {
                                    nodes[value].interior = false;
                                }
                            }
                        }

                        //merge case 3: if node i is first on route, and node j is last on route
                        if (nodes[currenti].firstonroute == true && nodes[currentj].lastonroute == true)
                        {
                            List<int> oldroute3 = new List<int>(); //currrent old route
                            oldroute3 = routes[nodes[currenti].routeon];//assign current old route to the route that i is currently on
                            int oldroute3index = nodes[currenti].routeon; //to store index to delete after foreach loop
                            for (int k = oldroute3.Count - 1; k >= 0; k--) //for each element in the old route, add it to the current route j
                            { routes[nodes[currentj].routeon].Add(oldroute3[k]); }
                            oldroute3.Clear();//clear the old route that j was on
                            routes[oldroute3index] = oldroute3; //assign the cleared route to the routes list
                            //update
                            List<int> newroute3 = new List<int>(); //create the current new route, with both i and j on it
                            newroute3 = routes[nodes[currentj].routeon];//assign new route 3 based on what current route j was on
                            int jon = nodes[currentj].routeon; //record the index of the route that current j is on
                            newroute3.ForEach(delegate(int value) { nodes[value].routeon = jon; });//update the routeon for each element in the new route
                            foreach (var value in newroute3) //update firston, laston and interior bools
                            {
                                if (newroute3.IndexOf(value) == 0)
                                {
                                    nodes[value].firstonroute = true;
                                }
                                else
                                {
                                    nodes[value].firstonroute = false;
                                }

                                if (newroute3.IndexOf(value) == newroute3.Count-1)
                                {
                                    nodes[value].lastonroute = true;
                                }
                                else
                                {
                                    nodes[value].lastonroute = false;
                                }
                                if (nodes[value].firstonroute == false && nodes[value].lastonroute == false)
                                {
                                    nodes[value].interior = true;
                                }
                                else
                                {
                                    nodes[value].interior = false;
                                }
                            }
                        }

                        //merge case 4: if node i and j are both first on the route
                        if (nodes[currenti].firstonroute == true && nodes[currentj].firstonroute == true)
                        {
                            List<int> oldroute4j = new List<int>(); //current old route j
                            oldroute4j = routes[nodes[currentj].routeon]; //assign the old route as the route that current j is on
                            oldroute4j.Reverse(); //reverse the elements of j before merging so i,j is in the route
                            routes[nodes[currentj].routeon] = oldroute4j; //update the routes matrix with the reversed current j route
                            List<int> oldroute4i = new List<int>(); //current old route i
                            oldroute4i = routes[nodes[currenti].routeon]; //assign the old route as the route that current i is on
                            int oldroute4index = nodes[currenti].routeon; //to store index in that current i is on to delete after foreach loop
                            for (int k = oldroute4i.Count - 1; k >= 0; k--) // for each element in the old route that contains j(now that is reversed) add the old route i elements 
                            { routes[nodes[currentj].routeon].Add(oldroute4i[k]); }
                            oldroute4i.Clear();//clear old route i 
                            routes[oldroute4index] = oldroute4i; //update the route that current i was on with the cleared route list
                            //update
                            List<int> newroute4 = new List<int>(); //new route that now contains both i and j
                            newroute4 = routes[nodes[currentj].routeon]; //assign new route from routes matrix
                            int jon = nodes[currentj].routeon; //record the current routeon for j 
                            newroute4.ForEach(delegate(int value) { nodes[value].routeon = jon; }); //for each element on the new route, update the routeon to match
                            foreach (var value in newroute4) //update firston, laston and interior bools
                            {
                                if (newroute4.IndexOf(value) == 0)
                                {
                                    nodes[value].firstonroute = true;
                                }
                                else
                                {
                                    nodes[value].firstonroute = false;
                                }

                                if (newroute4.IndexOf(value) == newroute4.Count-1)
                                {
                                    nodes[value].lastonroute = true;
                                }
                                else
                                {
                                    nodes[value].lastonroute = false;
                                }
                                if (nodes[value].firstonroute == false && nodes[value].lastonroute == false)
                                {
                                    nodes[value].interior = true;
                                }
                                else
                                {
                                    nodes[value].interior = false;
                                }
                            }
                        }
                    }

                }

            
            }
            //put the single nodes on routes
            int number=0;
            foreach (var singlenode in nodes)
            {
                if (!singlenode.assigned && number!=0)
                {
                    List<int> missing = new List<int>();
                    missing.Add(number);
                    routes.Add(missing);
                }
                number++;
            }

            network.routes = routes;
            //total cost calculation 
            float objective = 0;
            int numofroutes = routes.Count;
            for (int i = 0; i < numofroutes; i++)
            {
                List<int> singleroute = new List<int>();
                singleroute = routes[i];

                for (int j = 0; j < singleroute.Count-1; j++)
                {

                    objective = objective + costmatrix[singleroute[j], singleroute[j+1]];

                    if (j == 0)
                    {                   
                        objective = objective + costmatrix[0, singleroute[j]];
                    }
                    if (j == singleroute.Count - 2)
                    {
                        objective = objective + costmatrix[0, singleroute[j+1]];
                    }

                }
                network.objective = objective;
            }

        }
    }
}
